﻿using System;
using System.Collections.Generic;

using System.Text;

namespace rkShared
{
	

	//All math related functions will be in this class
	//
	public class UMath
	{

		public enum INTERSECTION{
			INTERSECTS,
			FRONT,
			BEHIND
		}
		public static void CrossProduct(Vector3 output,Vector3 v1,Vector3 v2)
		{
			output.x = v1.y * v2.z - v1.z * v2.y;
			output.y = v1.z * v2.x - v1.x * v2.z;
			output.z = v1.x * v2.y - v1.y * v2.x;
		}
		
        //Normalize v put the result in v
        public static Vector3 Normalize(ref Vector3 output,Vector3 v)
		{
			double magnitude = Math.Sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
			output.x = v.x / (float)magnitude;
			output.y = v.y / (float)magnitude;
			output.z = v.z / (float)magnitude;
			return output;
		}
		public static Vector3 Normal(Vector3[] vPolygon)					
		{														
			Vector3 vVector1 = vPolygon[1] - vPolygon[0];
			Vector3 vVector2 = vPolygon[2] - vPolygon[0];

			//find the normal with cross product
			Vector3 vNormal = new Vector3();
			CrossProduct(vNormal,vVector1,vVector2);		

			//normalize 
			Normalize(ref vNormal,vNormal);						
			return vNormal;										
		}

        //---------------------------------------
        //Calculates the dot product of 2 vectors
        //---------------------------------------
        public static float Dot( Vector3 pVector1, Vector3 pVector2) 
        {
	        return ( (pVector1.x * pVector2.x) + (pVector1.y * pVector2.y) + (pVector1.z * pVector2.z) );
        }
        //--------------------------------------
        //Finds the distance between two vectors
        //--------------------------------------
        public static float Distance(Vector3 pPoint1,Vector3 pPoint2)
        {
	        double distance = Math.Sqrt( (pPoint1.x - pPoint2.x) * (pPoint1.x - pPoint2.x) +
		        				        (pPoint1.y - pPoint2.y) * (pPoint1.y - pPoint2.y) +
			        			        (pPoint1.z - pPoint2.z) * (pPoint1.z - pPoint2.z) );
	        return (float)distance;
        }
        //---------------------------------------------------------------------
        //This finds the point that is closest to pPoint which is on line pA-pB
        //---------------------------------------------------------------------
        public static Vector3 ClosestPointOnLine(Vector3 pA,Vector3 pB,Vector3 pPoint)
        {
	        Vector3 vVector1 = pPoint - pA;
	        Vector3 vVector2 = pB - pA;


	        Normalize(ref vVector2,vVector2);
        		
            float d = Distance(pA,pB);
	        float t = Dot(vVector2, vVector1);
            if (t <= 0) 
		        return pA;
            if (t >= d) 
		        return pB;
           
	        Vector3 vVector3 = vVector2 * (t);
	        Vector3 vClosestPoint = vVector3 + pA ;
	        return vClosestPoint;
        }

        //---------------------------------------------------------
        //Returns the shortest distance between a plane and a point
        //---------------------------------------------------------
        public static float DistanceFromPointToPlane(Vector3 pNormal,Vector3 pPlane,Vector3 pPoint)
        {
	        float originDistance = PlaneDistance(pNormal,pPlane);

	        return Dot(pNormal,pPoint) + originDistance;
        }
        //-----------------------------------------------
        //Returns the distance of the plane to the origin
        //-----------------------------------------------
        public static float PlaneDistance(Vector3 pNormal,Vector3 pPoint)
        {	
	        return - ((pNormal.x * pPoint.x) + (pNormal.y * pPoint.y) + (pNormal.z * pPoint.z));
        }											
        //--------------------------
        //This finds the intersection of line and polygon
        public static bool IntersectedPlane(Vector3[] vPoly, Vector3[] vLine, out Vector3 vNormal,out float originDistance)
        {
	        float distance1=0, distance2=0;						
        			
	        vNormal = Normal(vPoly);							

	        originDistance = PlaneDistance(vNormal, vPoly[0]);

	        // Get the distance from point1 from the plane using: Ax + By + Cz + D = (The distance from the plane)

	        distance1 = ((vNormal.x * vLine[0].x)  +					// Ax +
		                 (vNormal.y * vLine[0].y)  +					// Bx +
				         (vNormal.z * vLine[0].z)) + originDistance;	// Cz + D
        	
	        // Get the distance from point2 from the plane using Ax + By + Cz + D = (The distance from the plane)
        	
	        distance2 = ((vNormal.x * vLine[1].x)  +					// Ax +
		                 (vNormal.y * vLine[1].y)  +					// Bx +
				         (vNormal.z * vLine[1].z)) + originDistance;	// Cz + D

	        // Now that we have 2 distances from the plane, if we times them together we either
	        // get a positive or negative number.  If it's a negative number, that means we collided!
	        // This is because the 2 points must be on either side of the plane (IE. -1 * 1 = -1).

	        if(distance1 * distance2 >= 0)			// Check to see if both point's distances are both negative or both positive
	           return false;						// Return false if each point has the same sign.  -1 and 1 would mean each point is on either side of the plane.  -1 -2 or 3 4 wouldn't...
        					
	        return true;							// The line intersected the plane, Return TRUE
        }
        ////Rotates vector1 with pivot Vector2 based on a axis
        ////This function takes in radians
        public static void RotateVector(float radian,ref Vector3 vector1,Vector3 vector2,float x,float y,float z)
        {
	        Vector3 vNewView = new Vector3();
	        Vector3 vView = new Vector3();	
        		
	        // Get our view vector (The direciton we are facing)
	        vView = vector1 - vector2;				
        	
	        // Calculate the sine and cosine of the angle once
	        float cosTheta = (float)Math.Cos(radian);
	        float sinTheta = (float)Math.Sin(radian);
        	
        	
	        //cosTheta=1-cosTheta;
	        vNewView.x  = (cosTheta + (1 - cosTheta) * x * x)		* vView.x;
	        vNewView.x += ((1 - cosTheta) * x * y - z * sinTheta)	* vView.y;
	        vNewView.x += ((1 - cosTheta) * x * z + y * sinTheta)	* vView.z;

	        vNewView.y  = ((1 - cosTheta) * x * y + z * sinTheta)	* vView.x;
	        vNewView.y += (cosTheta + (1 - cosTheta) * y * y)		* vView.y;
	        vNewView.y += ((1 - cosTheta) * y * z - x * sinTheta)	* vView.z;

	        vNewView.z  = ((1 - cosTheta) * x * z - y * sinTheta)	* vView.x;
	        vNewView.z += ((1 - cosTheta) * y * z + x * sinTheta)	* vView.y;
	        vNewView.z += (cosTheta + (1 - cosTheta) * z * z)		* vView.z;
        	
	        vector1 = vector2 + vNewView;
        }
        //------------------------------------------------------------------------------------------
        //This finds the angle between 2 vectors in radians with dot product A.B = |A|.|B|cos(theta)
        //------------------------------------------------------------------------------------------
        public static double AngleBetweenVectors(Vector3 pVector1,Vector3 pVector2)
        {							
	        //A.B
	        float dotProduct = Dot(pVector1,pVector2);				
	        //|A|*|B|
	        float vectorsMagnitude = pVector1.Magnitude() * pVector2.Magnitude();

	        //avoid division by zero
	        if(vectorsMagnitude==0)
		        return 0;
        	
	        //cos(theta) = A.B / |A|*|B|
	        double angle = Math.Acos( dotProduct / vectorsMagnitude );

	        //check the number
	        if(Double.IsNaN(angle))
		        return 0;
        		
	        return angle;
        }	
        //--------------------------------
        //This finds the intersection point
        //---------------------------------
        public static Vector3 IntersectionPoint(Vector3 vNormal, Vector3[] vLine, double distance)
        {
	        Vector3 vPoint = new Vector3();
			Vector3 vLineDir = new Vector3();					// Variables to hold the point and the line's direction
	        double Numerator = 0.0, Denominator = 0.0, dist = 0.0;

	        // 1)  First we need to get the vector of our line, Then normalize it so it's a length of 1
	        vLineDir = vLine[1] - vLine[0];		// Get the Vector of the line
	        Normalize(ref vLineDir,vLineDir);				// Normalize the lines vector


	        // 2) Use the plane equation (distance = Ax + By + Cz + D) to find the 
	        // distance from one of our points to the plane.
	        Numerator = - (vNormal.x * vLine[0].x +		// Use the plane equation with the normal and the line
				           vNormal.y * vLine[0].y +
				           vNormal.z * vLine[0].z + distance);

	        // 3) If we take the dot product between our line vector and the normal of the polygon,
	        Denominator = Dot(vNormal, vLineDir);		// Get the dot product of the line's vector and the normal of the plane
        				  
	        // Since we are using division, we need to make sure we don't get a divide by zero error
	        // If we do get a 0, that means that there are INFINATE points because the the line is
	        // on the plane (the normal is perpendicular to the line - (Normal.Vector = 0)).  
	        // In this case, we should just return any point on the line.

	        if( Denominator == 0.0)						// Check so we don't divide by zero
		        return vLine[0];						// Return an arbitrary point on the line

	        dist = Numerator / Denominator;				// Divide to get the multiplying (percentage) factor
        	
	        // Now, like we said above, we times the dist by the vector, then add our arbitrary point.

	        vPoint.x = (float)(vLine[0].x + (vLineDir.x * dist));
	        vPoint.y = (float)(vLine[0].y + (vLineDir.y * dist));
	        vPoint.z = (float)(vLine[0].z + (vLineDir.z * dist));

	        return vPoint;								// Return the intersection point
        }
        //------------------------------------------------------------------------------------------------
        //This checks if a point is inside a polygon casting rays to corners and adding the angles between
        //------------------------------------------------------------------------------------------------
        public static bool InsidePolygon(Vector3 pIntersection,Vector3[] Poly,long verticeCount)
        {
	        //this value needs carefull tweaking
			const double MATCH_FACTOR = 0.6;		
	        double Angle = 0.0;						
	        Vector3 vA, vB;						
        	
	        for (int i = 0; i < verticeCount; i++){	
		        vA = Poly[i] - pIntersection;			

		        vB = Poly[(i + 1) % verticeCount] - pIntersection;
        												
		        Angle += AngleBetweenVectors(vA, vB);	
	        }
        											
	        if(Angle >= (MATCH_FACTOR * (2.0 * Math.PI)) )	
		        return true;							
        		
	        return false;								
        }
        //------------------------------------------
        //This checks if a line intersects a polygon
        //------------------------------------------
        public static bool IntersectedPolygon(Vector3[] vPoly, Vector3[] vLine, int verticeCount,out Vector3 pIntersection)
        {
	        Vector3 vNormal = new Vector3();
	        float originDistance = 0;

	        
	        if(!IntersectedPlane(vPoly, vLine,   out vNormal,   out originDistance)){
				pIntersection = new Vector3();
		        return false;
			}

	        // Now that we have our normal and distance passed back from IntersectedPlane(), 
	        // we can use it to calculate the intersection point.  
	        pIntersection = IntersectionPoint(vNormal, vLine, originDistance);

	        // Now that we have the intersection point, we need to test if it's inside the polygon.
	        if(InsidePolygon(pIntersection, vPoly, verticeCount))
		        return true;							// We collided!	  Return success

	        return false;								// There was no collision, so return false
        }
        //----------------------------------------
        //This checks if a sphere intersects a box
        //----------------------------------------
        public static bool SphereOverlapsBox(Vector3 pSphere,float radius,Vector3 pBox,Vector3 pBoxDims)
        {
	        float distance = 0.0f, d;
	        Vector3 vDims = pBoxDims / 2;
	        // Eixo X
	        d = (pBox.x + vDims.x);
	        if( pSphere.x > d ){

		        d = pSphere.x - d;
		        distance += d * d;

	        }else{
		        d = (pBox.x - vDims.x);
		        if( pSphere.x < d ){
			        d = d - pSphere.x;
			        distance += d * d;
		        }
	        }
	        // Eixo Y
	        d = (pBox.y + vDims.y);
	        if( pSphere.y > d ){

		        d = pSphere.y - d;
		        distance += d * d;
	        }else{
		        d = (pBox.y - vDims.y);
		        if( pSphere.y < d ){
			        d = d - pSphere.y;
			        distance += d * d;
		        }
	        }
	        // Eixo Z
	        d = (pBox.z + vDims.z);
	        if( pSphere.z > d ){

		        d = pSphere.z - d;
		        distance += d * d;

	        }else{
		        d = (pBox.z - vDims.z);
		        if( pSphere.z < d ){
			        d = d - pSphere.z;
			        distance += d * d;

		        }
	        }

	        // if the squared distance is less than the square of the radius
	        return distance <= ( radius * radius );
        }
        bool PointInBox(Vector3 vPoint,Vector3 vBox,Vector3 vBoxDims)
        {
	        vBoxDims = vBoxDims / 2;
	        if( vPoint.x < vBox.x+vBoxDims.x && vPoint.x > vBox.x-vBoxDims.x &&
		        vPoint.y < vBox.y+vBoxDims.y && vPoint.y > vBox.y-vBoxDims.y &&
		        vPoint.z < vBox.z+vBoxDims.z && vPoint.z > vBox.z-vBoxDims.z )	
		        return true;
	        else return false;

        }
        public static bool PointInRectangle(Vector3 vPoint,Vector3 vRectangle,Vector3 vRectDims)
        {
	        vRectDims = vRectDims/2;
	        if( vPoint.x < vRectangle.x+vRectDims.x && vPoint.x > vRectangle.x-vRectDims.x &&
		        vPoint.z < vRectangle.z+vRectDims.z && vPoint.z > vRectangle.z-vRectDims.z )	
		        return true;
	        else return false;

        }
        //-----------------------------------
        //Checks if a ray intersects a sphere
        //-----------------------------------
        public static bool SphereRayCollision(Vector3[] vLine,Vector3 pCenter,float radius)
        {
	        Vector3 vVector = new Vector3(0,0,0);
        		
	        //find the closestPoint on the intersection ray to the center of the sphere
	        vVector = ClosestPointOnLine(vLine[0],vLine[1],pCenter);
	        //find the distance between these 2 vectors
	        float dist = Distance(pCenter,vVector);
        	
	        //if its smaller or equal than radius the ray passes through the sphere
	        if(Math.Abs(dist)<=radius) 
		        return true;
        	
	        return false;
        }
        public static bool stBoxRayCollision(Vector3[] vLine,Vector3 pMaxs,Vector3 pMins)
        {
	        Vector3[] vPoly = new Vector3[4];
	        Vector3 vIntersection = new Vector3(0,0,0);

	        //front
	        vPoly[0].x = pMins.x;
	        vPoly[0].y = pMaxs.y;
	        vPoly[0].z = pMaxs.z;

	        vPoly[1].x = pMins.x;
	        vPoly[1].y = pMins.y;
	        vPoly[1].z = pMaxs.z;

	        vPoly[2].x = pMaxs.x;
	        vPoly[2].y = pMins.y;
	        vPoly[2].z = pMaxs.z;

	        vPoly[3].x = pMaxs.x;
	        vPoly[3].y = pMaxs.y;
	        vPoly[3].z = pMaxs.z;
        	
	        if(IntersectedPolygon(vPoly,vLine,4,out vIntersection))
		        return true;
	        //right
	        vPoly[0].x = pMaxs.x;
	        vPoly[0].y = pMaxs.y;
	        vPoly[0].z = pMaxs.z;

	        vPoly[1].x = pMaxs.x;
	        vPoly[1].y = pMins.y;
	        vPoly[1].z = pMaxs.z;

	        vPoly[2].x = pMaxs.x;
	        vPoly[2].y = pMins.y;
	        vPoly[2].z = pMins.z;

	        vPoly[3].x = pMaxs.x;
	        vPoly[3].y = pMaxs.y;
	        vPoly[3].z = pMins.z;
        	
	        if(IntersectedPolygon(vPoly,vLine,4,out vIntersection))
		        return true;
	        //back
	        vPoly[0].x = pMaxs.x;
	        vPoly[0].y = pMaxs.y;
	        vPoly[0].z = pMins.z;

	        vPoly[1].x = pMaxs.x;
	        vPoly[1].y = pMins.y;
	        vPoly[1].z = pMins.z;

	        vPoly[2].x = pMins.x;
	        vPoly[2].y = pMins.y;
	        vPoly[2].z = pMins.z;

	        vPoly[3].x = pMins.x;
	        vPoly[3].y = pMaxs.y;
	        vPoly[3].z = pMins.z;
        	
	        if(IntersectedPolygon(vPoly,vLine,4,out vIntersection))
		        return true;
	        //left
	        vPoly[0].x = pMins.x;
	        vPoly[0].y = pMaxs.y;
	        vPoly[0].z = pMins.z;

	        vPoly[1].x = pMins.x;
	        vPoly[1].y = pMins.y;
	        vPoly[1].z = pMins.z;

	        vPoly[2].x = pMins.x;
	        vPoly[2].y = pMins.y;
	        vPoly[2].z = pMaxs.z;

	        vPoly[3].x = pMins.x;
	        vPoly[3].y = pMaxs.y;
	        vPoly[3].z = pMaxs.z;
        	
	        if(IntersectedPolygon(vPoly,vLine,4,out vIntersection))
		        return true;

	        //top
	        vPoly[0].x = pMins.x;
	        vPoly[0].y = pMaxs.y;
	        vPoly[0].z = pMins.z;

	        vPoly[1].x = pMins.x;
	        vPoly[1].y = pMaxs.y;
	        vPoly[1].z = pMaxs.z;

	        vPoly[2].x = pMaxs.x;
	        vPoly[2].y = pMaxs.y;
	        vPoly[2].z = pMaxs.z;

	        vPoly[3].x = pMaxs.x;
	        vPoly[3].y = pMaxs.y;
	        vPoly[3].z = pMins.z;
        	
	        if(IntersectedPolygon(vPoly,vLine,4,out vIntersection))
		        return true;

            //bottom
            vPoly[0].x = pMins.x;
            vPoly[0].y = pMins.y;
            vPoly[0].z = pMins.z;

            vPoly[1].x = pMins.x;
            vPoly[1].y = pMins.y;
            vPoly[1].z = pMaxs.z;

            vPoly[2].x = pMaxs.x;
            vPoly[2].y = pMins.y;
            vPoly[2].z = pMaxs.z;

            vPoly[3].x = pMaxs.x;
            vPoly[3].y = pMins.y;
            vPoly[3].z = pMins.z;

            if(IntersectedPolygon(vPoly,vLine,4,out vIntersection))
                return true;

	        return false;
        }
        //-----------------------------------------------------------------------
        //this tells if a sphere is behind,infront of or intersects a polygons plane
        //-----------------------------------------------------------------------
        public static INTERSECTION ClassifySphere(Vector3 pCenter,Vector3 pNormal,Vector3 pPoint, float radius, float pDistance)
        {
	        // First we need to find the distance our polygon plane is from the origin.
	        float d = (float)PlaneDistance(pNormal, pPoint);

	        // Here we use the famous distance formula to find the distance the center point
	        // of the sphere is from the polygon's plane.  
	        pDistance = (pNormal.x * pCenter.x + pNormal.y * pCenter.y + pNormal.z * pCenter.z + d);

	        // If the absolute value of the distance we just found is less than the radius, 
	        // the sphere intersected the plane.
	        if(Math.Abs(pDistance) < radius)
		        return INTERSECTION.INTERSECTS;
	        // Else, if the distance is greater than or equal to the radius, the sphere is
	        // completely in FRONT of the plane.
	        else if(pDistance >= radius)
		        return INTERSECTION.FRONT;
        	
	        // If the sphere isn't intersecting or in FRONT of the plane, it must be BEHIND
	        return INTERSECTION.BEHIND;
        }
        //-----------------------------------------------------------------
        //This checks the edges of the polygon if they intersect the sphere
        //-----------------------------------------------------------------
        public static bool EdgeSphereCollision(Vector3 pCenter,Vector3[] vPolygon, int vertexCount, float radius)
        {
	        Vector3 vPoint;

	        for(int i = 0; i < vertexCount; i++){
		        //get point on edge
		        vPoint = ClosestPointOnLine(vPolygon[i], vPolygon[(i + 1) % vertexCount],pCenter);
        		
		        //find dist to center of sphere
		        float dist = Distance(vPoint, pCenter);

		        //check if its smaller than radius if its then sphere intersects the edge
		        if(dist < radius)
			        return true;
	        }
	        return false;
        }
        //-------------------------------------------
        //Checks if the sphere intersects the polygon
        //-------------------------------------------
        public static bool SpherePolygonCollision(Vector3[] vPolygon,Vector3 pCenter, int vertexCount, float radius)
        {
        	
	        Vector3 vNormal = Normal(vPolygon);
	        Vector3 vCenter = pCenter;

	        float distance = 0.0f;

	        INTERSECTION classification = ClassifySphere(vCenter , vNormal, vPolygon[0], radius, distance);

	        if(classification == INTERSECTION.INTERSECTS){
		        Vector3 vOffset = vNormal * distance;

		        Vector3 vPosition = pCenter - vOffset;

		        if(InsidePolygon(vPosition, vPolygon, 3))
			        return true;	// intersects!
		        else{
			        if(EdgeSphereCollision(vCenter, vPolygon, vertexCount, radius))	{
				        return true;	// edge intersects!
			        }
		        }
	        }
	        return false;
        }
        //-----------------------------------------------------------
        //Finds the vector that should be added to the spheres center
        //-----------------------------------------------------------
        public static Vector3 stGetCollisionOffset( Vector3 pNormal, float radius, float distance)
        {
	        Vector3 vOffset = new Vector3(0, 0, 0);

	        //in front of the polygon
	        if(distance > 0){
		        float distanceOver = radius - distance;
		        vOffset = pNormal * distanceOver;
	        }
	        //behind the polygon
	        else{
        		
		        float distanceOver = radius + distance;
		        vOffset = pNormal * -distanceOver;
	        }
	        return vOffset;
        }
	}
}
